#include "aim_spin/AntiSpin.hpp"

AntiSpin::AntiSpin(){
    this->init_time = std::chrono::system_clock::now().time_since_epoch().count() / 1e9;
}

//当前版本是根据获取到的点的间隔来预测的，所以预测结果会与识别时间有关
void AntiSpin::anti(const cv::Point3f& t_cam, const serial_com::comm& msg,Eigen::Vector3d& cam_p){
    //从相机坐标系转到世界坐标系
    Eigen::Vector3d pw;//世界坐标系下的坐标
    Eigen::Quaterniond c2w;
    project2World(t_cam, msg.y, msg.x, pw, c2w); 
    //存储近60个x
    is_spin = true;
    float x_cuttent = pw(0);
    xs_history.push_front(x_cuttent);
    float current_time = (std::chrono::system_clock::now().time_since_epoch().count() / 1e9) - init_time;
    ts_history.push_front(current_time);
    if(xs_history.size() < HISTORY_SIZE){
        return;
    }
    if(!xs_history.empty()){
        xs_history.pop_back();
        ts_history.pop_back();
    }
    std::deque<float> v_points;//跳变点,2i为跳变点index，2i+1为差分值
    std::deque<float> t_points;//跳变点对应时间
    for(int i = 1; i < xs_history.size();i++){
        float v = xs_history[i] - xs_history[i-1];
        if(v > 150){
            v_points.push_back(i);
            v_points.push_back(v);
            t_points.push_back(ts_history[i]);
        }
    }
    // std::cout <<"v_points.size():" <<v_points.size() <<std::endl;
    if(v_points.size() < 6){
        is_spin = false;
    }
    for(int i = 2; i < v_points.size();i = i + 2){
        if(std::abs(v_points[i + 1] - v_points[i - 1]) > 80 ||
            std::abs(t_points[i/2] - t_points[i/2 - 1]) > 1 ||
               std::abs(t_points[i/2] - t_points[i/2 - 1]) < 0.05 ){
            is_spin = false;
        }
    }
    

    for(int i = 2; i < v_points.size();i = i + 2){
        int s = 0;
        float mid_len = v_points[i] - v_points[i - 2];
        float mid_sum = 0;
        for(int j = v_points[i - 2] + 1; j < v_points[i]; j++){
            if(xs_history[j]> xs_history[j - 1]){
                s = s + 1;
            }else{
                s = s - 1;
            }
            mid_sum += xs_history[j];
        }
        float ratio = (float)s / (float)mid_len;

        mid_avg = mid_sum / mid_len;
        // if(std::abs(ratio) < 0.75){
        //     is_spin = false;
        //     std::cout << std::abs(ratio) << std::endl;

        // }
    }   
    Eigen::Vector3d result(mid_avg,pw(1),pw(2)); 
    cam_p = c2w.conjugate() * result;  
}